#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "aes.h"

const int _KEY_LENGTHS[3] = {4,6,8}; //Nk
const int _KEY_ROUNDS[3] = {10,12,14}; //Nr

const uint8_t S_Box[16][16] = {
	{0x63,0x7c,0x77,0x7b,0xf2,0x6b,0x6f,0xc5,0x30,0x01,0x67,0x2b,0xfe,0xd7,0xab,0x76},
	{0xca,0x82,0xc9,0x7d,0xfa,0x59,0x47,0xf0,0xad,0xd4,0xa2,0xaf,0x9c,0xa4,0x72,0xc0},
	{0xb7,0xfd,0x93,0x26,0x36,0x3f,0xf7,0xcc,0x34,0xa5,0xe5,0xf1,0x71,0xd8,0x31,0x15},
	{0x04,0xc7,0x23,0xc3,0x18,0x96,0x05,0x9a,0x07,0x12,0x80,0xe2,0xeb,0x27,0xb2,0x75},
	{0x09,0x83,0x2c,0x1a,0x1b,0x6e,0x5a,0xa0,0x52,0x3b,0xd6,0xb3,0x29,0xe3,0x2f,0x84},
	{0x53,0xd1,0x00,0xed,0x20,0xfc,0xb1,0x5b,0x6a,0xcb,0xbe,0x39,0x4a,0x4c,0x58,0xcf},
	{0xd0,0xef,0xaa,0xfb,0x43,0x4d,0x33,0x85,0x45,0xf9,0x02,0x7f,0x50,0x3c,0x9f,0xa8},
	{0x51,0xa3,0x40,0x8f,0x92,0x9d,0x38,0xf5,0xbc,0xb6,0xda,0x21,0x10,0xff,0xf3,0xd2},
	{0xcd,0x0c,0x13,0xec,0x5f,0x97,0x44,0x17,0xc4,0xa7,0x7e,0x3d,0x64,0x5d,0x19,0x73},
	{0x60,0x81,0x4f,0xdc,0x22,0x2a,0x90,0x88,0x46,0xee,0xb8,0x14,0xde,0x5e,0x0b,0xdb},
	{0xe0,0x32,0x3a,0x0a,0x49,0x06,0x24,0x5c,0xc2,0xd3,0xac,0x62,0x91,0x95,0xe4,0x79},
	{0xe7,0xc8,0x37,0x6d,0x8d,0xd5,0x4e,0xa9,0x6c,0x56,0xf4,0xea,0x65,0x7a,0xae,0x08},
	{0xba,0x78,0x25,0x2e,0x1c,0xa6,0xb4,0xc6,0xe8,0xdd,0x74,0x1f,0x4b,0xbd,0x8b,0x8a},
	{0x70,0x3e,0xb5,0x66,0x48,0x03,0xf6,0x0e,0x61,0x35,0x57,0xb9,0x86,0xc1,0x1d,0x9e},
	{0xe1,0xf8,0x98,0x11,0x69,0xd9,0x8e,0x94,0x9b,0x1e,0x87,0xe9,0xce,0x55,0x28,0xdf},
	{0x8c,0xa1,0x89,0x0d,0xbf,0xe6,0x42,0x68,0x41,0x99,0x2d,0x0f,0xb0,0x54,0xbb,0x16}
};

const uint8_t Inv_S_Box[16][16] = {
	{0x52,0x09,0x6a,0xd5,0x30,0x36,0xa5,0x38,0xbf,0x40,0xa3,0x9e,0x81,0xf3,0xd7,0xfb},
	{0x7c,0xe3,0x39,0x82,0x9b,0x2f,0xff,0x87,0x34,0x8e,0x43,0x44,0xc4,0xde,0xe9,0xcb},
	{0x54,0x7b,0x94,0x32,0xa6,0xc2,0x23,0x3d,0xee,0x4c,0x95,0x0b,0x42,0xfa,0xc3,0x4e},
	{0x08,0x2e,0xa1,0x66,0x28,0xd9,0x24,0xb2,0x76,0x5b,0xa2,0x49,0x6d,0x8b,0xd1,0x25},
	{0x72,0xf8,0xf6,0x64,0x86,0x68,0x98,0x16,0xd4,0xa4,0x5c,0xcc,0x5d,0x65,0xb6,0x92},
	{0x6c,0x70,0x48,0x50,0xfd,0xed,0xb9,0xda,0x5e,0x15,0x46,0x57,0xa7,0x8d,0x9d,0x84},
	{0x90,0xd8,0xab,0x00,0x8c,0xbc,0xd3,0x0a,0xf7,0xe4,0x58,0x05,0xb8,0xb3,0x45,0x06},
	{0xd0,0x2c,0x1e,0x8f,0xca,0x3f,0x0f,0x02,0xc1,0xaf,0xbd,0x03,0x01,0x13,0x8a,0x6b},
	{0x3a,0x91,0x11,0x41,0x4f,0x67,0xdc,0xea,0x97,0xf2,0xcf,0xce,0xf0,0xb4,0xe6,0x73},
	{0x96,0xac,0x74,0x22,0xe7,0xad,0x35,0x85,0xe2,0xf9,0x37,0xe8,0x1c,0x75,0xdf,0x6e},
	{0x47,0xf1,0x1a,0x71,0x1d,0x29,0xc5,0x89,0x6f,0xb7,0x62,0x0e,0xaa,0x18,0xbe,0x1b},
	{0xfc,0x56,0x3e,0x4b,0xc6,0xd2,0x79,0x20,0x9a,0xdb,0xc0,0xfe,0x78,0xcd,0x5a,0xf4},
	{0x1f,0xdd,0xa8,0x33,0x88,0x07,0xc7,0x31,0xb1,0x12,0x10,0x59,0x27,0x80,0xec,0x5f},
	{0x60,0x51,0x7f,0xa9,0x19,0xb5,0x4a,0x0d,0x2d,0xe5,0x7a,0x9f,0x93,0xc9,0x9c,0xef},
	{0xa0,0xe0,0x3b,0x4d,0xae,0x2a,0xf5,0xb0,0xc8,0xeb,0xbb,0x3c,0x83,0x53,0x99,0x61},
	{0x17,0x2b,0x04,0x7e,0xba,0x77,0xd6,0x26,0xe1,0x69,0x14,0x63,0x55,0x21,0x0c,0x7d}
};

const uint32_t Rcon[11] = {0,0x01000000,0x02000000,0x04000000,0x08000000,0x10000000,0x20000000,0x40000000,0x80000000,0x1b000000,0x36000000};

inline uint8_t SubByte(uint8_t in)
{
	return S_Box[(in & 0xF0) >> 4][in & 0xF];
}

inline void SubBytes(uint8_t *in)
{
	int i,j;
	for(i = 0; i < 4; i++)
	{
		for(j = 0; j < 4; j++)
		{
			in[i*4+j] = SubByte(in[i*4+j]);
		}
	}
}

inline uint32_t SubWord(uint32_t in)
{
	int i;
	uint32_t temp = 0;
	
	for(i = 0; i < 4; i++)
		temp |= SubByte((in & (0xFF << (i << 3))) >> (i << 3)) << (i << 3);
	
	return temp;
}

inline uint32_t RotWord(uint32_t in)
{
	return (in << 8) | (in >> 24);
	//return (in >> 8) | (i << 24);
}

inline void print_state(const char* label, uint8_t *state, int round)
{
	int i;
	printf("round[%2d].%s\t\t",round,label);
	for(i = 0; i < 4*BLOCK_LENGTH; i++)
	{
		printf("%.2x",state[i]);
	}
	printf("\n");
}

inline void print_sched_state(const char* label, uint32_t *state, int round)
{
	int i;
	printf("round[%2d].%s\t\t",round,label);
	for(i = 0; i < BLOCK_LENGTH; i++)
	{
		printf("%.8x",state[i]);
	}
	printf("\n");
}

inline void AddRoundKey(uint8_t *state,uint32_t *round_keys,int start)
{
	int i;
	for(i = 0; i < 4*BLOCK_LENGTH; i+=4)
	{
		//memcpy(&temp,state+i,4);
		/*temp = (state[i] << 24) | (state[i+1] << 16) | (state[i+2] << 8) | state[i+3];
		temp ^= round_keys[start+i/4];
		state[0] = temp >> 24;
		state[1] = (0x00FF0000 & temp) >> 16;
		state[2] = (0x0000FF00 & temp) >> 8;
		state[3] = 0x000000FF & temp;*/
		state[i] ^= round_keys[start+i/4] >> 24;
		state[i+1] ^= (round_keys[start+i/4] & 0xFF0000) >> 16;
		state[i+2] ^= (round_keys[start+i/4] & 0xFF00) >> 8;
		state[i+3] ^= round_keys[start+i/4] & 0xFF;
		//memcpy(state+i,&temp,4);
	}
}

inline void LShift(uint8_t* in, int row)
{
	int i;
	uint8_t temp;
	for(i = 0; i < row; i++)
	{
		temp = in[row];
		in[row] = in[row+4];
		in[row+4] = in[row+8];
		in[row+8] = in[row+12];
		in[row+12] = temp;
	}
}

inline void RShift(uint8_t* in, int row)
{
	int i;
	uint8_t temp;
	for(i = 0; i < row; i++)
	{
		temp = in[row+12];
		in[row+12] = in[row+8];
		in[row+8] = in[row+4];
		in[row+4] = in[row];
		in[row] = temp;
	}
}

inline void ShiftRows(uint8_t *in)
{
	int i;
	for(i = 1; i < 4; i++)
	{
		LShift(in,i);
	}
}

inline uint8_t xtime(uint8_t in, uint8_t factor)
{
	uint16_t temp;
	if(factor == 1)
	{
		return in;
	}
	if(factor > 2)
	{
		in = xtime(in, factor >> 1);
	}
	temp = in << 1;
	if(temp & 0x100) temp ^= 0x1b;
	return (uint8_t)temp;
}

uint8_t multiply(uint8_t a, uint8_t b)
{
	uint8_t constant = 0x80;
	uint8_t result = 0;
	while(constant)
	{
		if(constant & b)
		{
			result ^= xtime(a,constant);
		}
		constant >>= 1;
	}
	return result;
}

inline void MixColumns(uint8_t *in)
{
	int i;
	uint8_t temp[4];
	for(i = 0; i < 4; i++)
	{
		temp[0] = multiply(0x02,in[4*i]) ^ multiply(0x03,in[4*i+1]) ^ in[4*i+2] ^ in[4*i+3];
		temp[1] = in[4*i] ^ multiply(0x02,in[4*i+1]) ^ multiply(0x03,in[4*i+2]) ^ in[4*i+3];
		temp[2] = in[4*i] ^ in[4*i+1] ^ multiply(0x02,in[4*i+2]) ^ multiply(0x03,in[4*i+3]);
		temp[3] = multiply(0x03,in[4*i]) ^ in[4*i+1] ^ in[4*i+2] ^ multiply(0x02,in[4*i+3]);
		
		in[4*i] = temp[0];
		in[4*i+1] = temp[1];
		in[4*i+2] = temp[2];
		in[4*i+3] = temp[3];
	}
}

inline void InvMixColumns(uint8_t *in)
{
	int i;
	uint8_t temp[4];
	for(i = 0; i < 4; i++)
	{
		temp[0] = multiply(0x0e,in[4*i]) ^ multiply(0x0b,in[4*i+1]) ^ multiply(0x0d,in[4*i+2]) ^ multiply(0x09,in[4*i+3]);
		temp[1] = multiply(0x09,in[4*i]) ^ multiply(0x0e,in[4*i+1]) ^ multiply(0x0b,in[4*i+2]) ^ multiply(0x0d,in[4*i+3]);
		temp[2] = multiply(0x0d,in[4*i]) ^ multiply(0x09,in[4*i+1]) ^ multiply(0x0e,in[4*i+2]) ^ multiply(0x0b,in[4*i+3]);
		temp[3] = multiply(0x0b,in[4*i]) ^ multiply(0x0d,in[4*i+1]) ^ multiply(0x09,in[4*i+2]) ^ multiply(0x0e,in[4*i+3]);
		
		in[4*i] = temp[0];
		in[4*i+1] = temp[1];
		in[4*i+2] = temp[2];
		in[4*i+3] = temp[3];
	}
}

inline void InvShiftRows(uint8_t *in)
{
	int i;
	for(i = 1; i < 4; i++)
	{
		RShift(in,i);
	}
}

inline uint8_t InvSubByte(uint8_t in)
{
	return Inv_S_Box[(in & 0xF0) >> 4][in & 0xF];
}

inline void InvSubBytes(uint8_t *in)
{
	int i,j;
	for(i = 0; i < 4; i++)
	{
		for(j = 0; j < 4; j++)
		{
			in[i*4+j] = InvSubByte(in[i*4+j]);
		}
	}
}

void key_expansion(Key* key)
{
	int i;
	uint32_t temp;
	
	for(i = 0; i < 8*key->length; i+=4)
	{
		//memcpy(key->key_expanded+i,key->key_val+4*i,4);
		key->key_expanded[i/4] = (key->key_val[i] << 24) | (key->key_val[i+1] << 16) | (key->key_val[i+2] << 8) | key->key_val[i+3];
	}
	
	for(i = key->length; i < key->expanded_length; i++)
	{
		temp = key->key_expanded[i-1];
		
		if(i % key->length == 0)
		{
			temp = SubWord(RotWord(temp)) ^ Rcon[i/key->length];
		}
		else if(key->length > 6 && i % key->length == 4)
		{
			temp = SubWord(temp);
		}
		
		key->key_expanded[i] = key->key_expanded[i-key->length] ^ temp;
	}
}

inline void key_expansion_inv(Key *key)
{
	int i,j,temp_ptr,round;
	uint8_t temp[4*BLOCK_LENGTH];
	
	/*for(round = 0; round < key->expanded_length; round+=4)
	{
		printf("i = %d\t",round/4);
		//print_sched_state("ik_sch",key->key_expanded+round*BLOCK_LENGTH,round);
		for(i = 0; i < 4; i++)
		{
			printf("%.8x",key->key_expanded[round+i]);
		}
		printf("\n");
	}*/
	
	memcpy(key->key_expanded_inv,key->key_expanded,key->expanded_length*sizeof(uint32_t));
	
	/*for(i = 0; i < 16; i++)
	{
		key->key_expanded_inv[i] = key->key_expanded[i];
	}*/
	
	//printf("key copied\n");
	
	/*for(round = 0; round < key->expanded_length; round+=4)
	{
		printf("i = %d\t",round/4);
		//print_sched_state("ik_sch",key->key_expanded+round*BLOCK_LENGTH,round);
		for(i = 0; i < 4; i++)
		{
			printf("%.8x",key->key_expanded_inv[round+i]);
		}
		printf("\n");
	}*/
	
	for(i = 1; i < key->rounds; i++)
	{
		temp_ptr = 0;
		for(j = i*BLOCK_LENGTH; j < (i+1)*BLOCK_LENGTH; j++)
		{
			temp[4*temp_ptr] = key->key_expanded_inv[j] >> 24;
			temp[4*temp_ptr+1] = (key->key_expanded_inv[j] & 0xFF0000) >> 16;
			temp[4*temp_ptr+2] = (key->key_expanded_inv[j] & 0xFF00) >> 8;
			temp[4*temp_ptr+3] = key->key_expanded_inv[j] & 0xFF;
			temp_ptr++;
		}
		//printf("temp[0] = %d\n",temp[0]);
		InvMixColumns(temp);
		//printf("temp[0] = %d\n",temp[0]);
		temp_ptr = 0;
		for(j = i*BLOCK_LENGTH; j < (i+1)*BLOCK_LENGTH; j++)
		{
			key->key_expanded_inv[j] = (temp[4*temp_ptr] << 24) | (temp[4*temp_ptr+1] << 16) | (temp[4*temp_ptr+2] << 8) | temp[4*temp_ptr+3];
			temp_ptr++;
		}
		//printf("i = %d\n",i);
	}
}

Key* new_key(KEY_TYPE type, uint8_t* key_val)
{
	Key* new_key;
	
	new_key = (Key*)malloc(sizeof(Key));
	
	new_key->key_val = (uint8_t*)malloc(KEY_BYTES(type));
	
	memcpy(new_key->key_val,key_val,KEY_BYTES(type));
	
	new_key->type = type;
	new_key->length = KEY_LENGTH(type);
	new_key->rounds = ROUNDS(type);
	
	new_key->expanded_length = BLOCK_LENGTH*(ROUNDS(type)+1);
	new_key->key_expanded = (uint32_t*)malloc(BLOCK_LENGTH*(ROUNDS(type)+1)*sizeof(uint32_t));
	new_key->key_expanded_inv = (uint32_t*)malloc(BLOCK_LENGTH*(ROUNDS(type)+1)*sizeof(uint32_t));
	
	key_expansion(new_key);
	//key_expansion_inv(new_key);
	
	return new_key;
}

Key* new_key_dec(KEY_TYPE type, uint8_t* key_val)
{
	Key* new_key;
	
	new_key = (Key*)malloc(sizeof(Key));
	
	new_key->key_val = (uint8_t*)malloc(KEY_BYTES(type));
	
	memcpy(new_key->key_val,key_val,KEY_BYTES(type));
	
	new_key->type = type;
	new_key->length = KEY_LENGTH(type);
	new_key->rounds = ROUNDS(type);
	
	new_key->expanded_length = BLOCK_LENGTH*(ROUNDS(type)+1);
	new_key->key_expanded = (uint32_t*)malloc(BLOCK_LENGTH*(ROUNDS(type)+1)*sizeof(uint32_t));
	new_key->key_expanded_inv = (uint32_t*)malloc(BLOCK_LENGTH*(ROUNDS(type)+1)*sizeof(uint32_t));
	
	key_expansion(new_key);
	key_expansion_inv(new_key);
	
	return new_key;
}

void aes_encrypt(uint8_t *input, Key *key)
{
	int round;
	uint8_t state[4*BLOCK_LENGTH];
	memcpy(state,input,4*BLOCK_LENGTH);
	//printf("key:\t");
	/*for(round = 0; round < 16; round++)
	{
		printf("%.2x",key->key_val[round]);
	}
	printf("\n");
	print_state("input",state,round);
	print_sched_state("k_sch",key->key_expanded,round); //WRONG*/
	AddRoundKey(state,key->key_expanded,0);
	
	for(round = 1; round < key->rounds; round++)
	{
		//print_state("start",state,round);
		SubBytes(state);
		//print_state("s_box",state,round);
		ShiftRows(state);
		//print_state("s_row",state,round);
		MixColumns(state);
		//print_state("m_col",state,round); //WRONG
		//print_sched_state("k_sch",key->key_expanded+round*BLOCK_LENGTH,round);
		AddRoundKey(state,key->key_expanded,round*BLOCK_LENGTH);
	}
	
	SubBytes(state);
	//print_state("s_box",state,round);
	ShiftRows(state);
	//print_state("s_row",state,round);
	AddRoundKey(state,key->key_expanded,round*BLOCK_LENGTH);
	//print_sched_state("k_sch",key->key_expanded+round*BLOCK_LENGTH,round);
	//print_state("output",state,round);
	memcpy(input,state,4*BLOCK_LENGTH);
}

void aes_decrypt(uint8_t *input, Key *key)
{
	int round;
	uint8_t state[4*BLOCK_LENGTH];
	memcpy(state,input,4*BLOCK_LENGTH);

	//printf("key:\t");
	/*for(round = 0; round < 16; round++)
	{
		printf("%.2x",key->key_val[round]);
	}
	printf("\n");
	print_state("iinput",state,0);
	print_sched_state("ik_sch",key->key_expanded+10*BLOCK_LENGTH,10); //WRONG*/
	
	AddRoundKey(state,key->key_expanded,BLOCK_LENGTH*key->rounds);
	
	for(round = key->rounds - 1; round > 0 ; round--)
	{
		//print_state("istart",state,10-round);
		InvShiftRows(state);
		//print_state("is_row",state,10-round);
		InvSubBytes(state);
		//print_state("is_box",state,10-round);
		//print_sched_state("ik_sch",key->key_expanded+round*BLOCK_LENGTH,10-round);
		AddRoundKey(state,key->key_expanded,round*BLOCK_LENGTH);
		//print_state("ik_add",state,10-round);
		InvMixColumns(state);
		//print_state("im_col",state,10-round);
	}
	
	InvShiftRows(state);
	//print_state("is_row",state,10);
	InvSubBytes(state);
	//print_state("is_box",state,10);
	AddRoundKey(state,key->key_expanded,0);
	//print_sched_state("ik_sch",key->key_expanded,10-round);
	//print_state("ioutput",state,round);
	memcpy(input,state,4*BLOCK_LENGTH);
}

void aes_decrypt_equiv(uint8_t *input, Key *key)
{
	int round;
	uint8_t state[4*BLOCK_LENGTH];
	memcpy(state,input,4*BLOCK_LENGTH);
	
	//printf("key:\t");
	/*for(round = 0; round < 16; round++)
	{
		printf("%.2x",key->key_val[round]);
	}
	printf("\n");*/
	
	
	//print_sched_state("ik_sch",key->key_expanded_inv,10); //WRONG
	//print_state("iinput",state,0);
	AddRoundKey(state,key->key_expanded_inv,BLOCK_LENGTH*key->rounds);
	
	for(round = key->rounds - 1; round > 0 ; round--)
	{
		//print_state("istart",state,10-round);
		InvSubBytes(state);
		//print_state("is_box",state,10-round);
		InvShiftRows(state);
		//print_state("is_row",state,10-round);
		InvMixColumns(state);
		//print_state("im_col",state,10-round);
		//print_sched_state("ik_sch",key->key_expanded_inv+(10-round)*BLOCK_LENGTH,10-round);
		AddRoundKey(state,key->key_expanded_inv,round*BLOCK_LENGTH);
	}
	
	InvSubBytes(state);
	InvShiftRows(state);
	AddRoundKey(state,key->key_expanded_inv,0);
	//print_state("ioutput",state,round);
	memcpy(input,state,4*BLOCK_LENGTH);
}
